#include "voxelization.h"
#include <cmath>
#include <map>
#include <cstdlib>
#include <sstream>
#include <fstream>
#include <string>

const int R = 32;
const double PI = atan(1.) * 4;

bool voxelization::intersect(BoundingBox a, BoundingBox b) {
	float3 a_min = a.getMin();
	float3 a_max = a.getMax();
	float3 b_min = b.getMin();
	float3 b_max = b.getMax();
	if (a_min.x > b_max.x)
		return false;
	if (a_max.x < b_min.x)
		return false;
	if (a_min.y > b_max.y)
		return false;
	if (a_max.y < b_min.y)
		return false;
	if (a_min.z > b_max.z)
		return false;
	if (a_max.z < b_min.z)
		return false;
	return true;
}

float3 voxelization::ABG_compute(float j, float i, float x0, float y0, float x1,
		float y1, float x2, float y2) {
	float3 result;
	float alpha = (float) ((y1 - y2) * j + (x2 - x1) * i + x1 * y2 - x2 * y1)
			/ ((y1 - y2) * x0 + (x2 - x1) * y0 + x1 * y2 - x2 * y1);

	float beta = (float) ((y2 - y0) * j + (x0 - x2) * i + x2 * y0 - x0 * y2)
			/ ((y2 - y0) * x1 + (x0 - x2) * y1 + x2 * y0 - x0 * y2);

	float gama = (float) ((y0 - y1) * j + (x1 - x0) * i + x0 * y1 - x1 * y0)
			/ ((y0 - y1) * x2 + (x1 - x0) * y2 + x0 * y1 - x1 * y0);

	result.x = alpha;
	result.y = beta;
	result.z = gama;
	return result;
}

void voxelization::surface_voxelize(float3 a, float3 b, float3 c,
		vector<BoundingBox> &voxellist, int resolution) {
     
	float3 normal;
	float3 v1, v2;
	float alpha, beta, gama;
	float x_min, x_max, y_min, y_max, z_min, z_max;
	x_min = min(c.x, min(a.x, b.x));
	x_max = max(c.x, max(a.x, b.x));
	y_min = min(c.y, min(a.y, b.y));
	y_max = max(c.y, max(a.y, b.y));
	z_min = min(c.z, min(a.z, b.z));
	z_max = max(c.z, max(a.z, b.z));

	v1 = a - b;
	v2 = c - b;
	normal = v1 ^ v2;

	float3 vmin, vmax;
	vmin = voxellist[0].getMin();
	vmax = voxellist[voxellist.size() - 1].getMax();
   // cout<<voxellist.size()<<endl;

	float center_minx, center_maxx, center_miny, center_maxy, center_minz,
			center_maxz;
    float q_ax, q_ay, q_az, q_bx, q_by, q_bz, q_cx, q_cy, q_cz;
    
    q_ax = (int)(a.x-vmin.x)+voxellist[0].getCenter().x;
    q_ay = (int)(a.y-vmin.y)+voxellist[0].getCenter().y;
    q_az = (int)(a.z-vmin.z)+voxellist[0].getCenter().z;
    
    q_bx = (int)(b.x-vmin.x)+voxellist[0].getCenter().x;
    q_by = (int)(b.y-vmin.y)+voxellist[0].getCenter().y;
    q_bz = (int)(b.z-vmin.z)+voxellist[0].getCenter().z;
    
    q_cx = (int)(c.x-vmin.x)+voxellist[0].getCenter().x;
    q_cy = (int)(c.y-vmin.y)+voxellist[0].getCenter().y;
    q_cz = (int)(c.z-vmin.z)+voxellist[0].getCenter().z;
    
    center_minz = (int) (z_min - vmin.z)+ voxellist[0].getCenter().z;
    center_maxz = (int) (z_max - vmin.z)+ voxellist[0].getCenter().z;
    center_miny = (int) (y_min - vmin.y)+ voxellist[0].getCenter().y;
    center_maxy = (int) (y_max - vmin.y)+ voxellist[0].getCenter().y;
    center_minx = (int) (x_min - vmin.x)+ voxellist[0].getCenter().x;
    center_maxx = (int) (x_max - vmin.x)+ voxellist[0].getCenter().x;

	/*project to YOZ plane*/
	if (abs(normal.x) >= abs(normal.y) && abs(normal.x) >= abs(normal.z)) {
        
        for (float i = center_minz; i < center_maxz; i = i + 1) {
			for (float j = center_miny; j < center_maxy; j = j + 1) {

                float3 abg = ABG_compute(j, i, q_ay, q_az, q_by, q_bz, q_cy, q_cz);
				alpha = abg.x;
				beta = abg.y;
				gama = abg.z;

				if (alpha >= 0 && beta >= 0 && gama >= 0) {

					float init_x = alpha * a.x + beta * b.x + gama * c.x;
					if (init_x <= vmax.x &&i<= vmax.z && j<=vmax.y &&init_x>=vmin.x && i>=vmin.z && j>=vmin.y) {
                        float voxel_x = (int) (init_x
                                                - voxellist[0].getCenter().x) 
                        + voxellist[0].getCenter().x;
                        
                        int index =
                        (int) (voxel_x - voxellist[0].getCenter().x)* resolution * resolution
                        + (int) (j - voxellist[0].getCenter().y)* resolution
                        + (int) (i - voxellist[0].getCenter().z);
                    
						voxellist[index].setFlag();
					}

				}
			}
		}
	}
	/*project to XOZ plane*/
	else if (abs(normal.y) >= abs(normal.x) && abs(normal.y) >= abs(normal.z)) {
        
        for (float i = center_minz; i < center_maxz; i = i + 1) {
			for (float j = center_minx; j < center_maxx; j = j + 1) {

                float3 abg = ABG_compute(j, i, q_ax, q_az, q_bx, q_bz, q_cx, q_cz);
				alpha = abg.x;
				beta = abg.y;
				gama = abg.z;
				if (alpha >= 0 && beta >= 0 && gama >= 0) {
					float init_y = alpha * a.y + beta * b.y + gama * c.y;
					if (init_y <= vmax.y && i<=vmax.z && j<=vmax.x && init_y>=vmin.y && i>= vmin.z && j>=vmin.x) {
                        float voxel_y = (int) (init_y
                                                - voxellist[0].getCenter().y)
                        + voxellist[0].getCenter().y;
                        
						int index = (int) (j - voxellist[0].getCenter().x)* resolution * resolution
                        + (int) (voxel_y - voxellist[0].getCenter().y)* resolution
                        + (int) (i - voxellist[0].getCenter().z);
                    
						voxellist[index].setFlag();
					}

				}
			}
		}
	}

	/*project to XOY plane*/
	else if (abs(normal.z) >= abs(normal.x) && abs(normal.z) >= abs(normal.y)) {
        for (float i = center_miny; i < center_maxy; i = i + 1) {
			for (float j = center_minx; j < center_maxx; j = j + 1) {
                
                float3 abg = ABG_compute(j, i, q_ax, q_ay, q_bx, q_by, q_cx, q_cy);
				alpha = abg.x;
				beta = abg.y;
				gama = abg.z;
				if (alpha >= 0 && beta >= 0 && gama >= 0) {
					float init_z = alpha * a.z + beta * b.z + gama * c.z;
					if (init_z <= vmax.z && i<=vmax.y && j<=vmax.x && init_z>=vmin.z &&i>=vmin.y && j>=vmin.x) {
                        
                        float voxel_z = (int) (init_z
                                                - voxellist[0].getCenter().z)+ voxellist[0].getCenter().z;
						int index = (int) (j - voxellist[0].getCenter().x)* resolution * resolution
                        + (int) (i - voxellist[0].getCenter().y)* resolution
                        + (int) (voxel_z - voxellist[0].getCenter().z);
        
						voxellist[index].setFlag();
					}

				}
			}
		}
	}

}

vector<BoundingBox> voxelization::voxelize(int resolution) {
   

	vector<float3> nor_vertex;
	vector<BoundingBox> voxellist;

	float3 vmin, vmax;
	float x_length;
	float y_length;
	float z_length;
	float x_step, y_step, z_step;

	nor_vertex = vertex_list;
   
    vmin.x = -resolution/2;
    vmin.y = -resolution/2;
    vmin.z = -resolution/2;
    x_step = y_step = z_step = 1;

	for (int i = 0; i < resolution; i++) {
		for (int j = 0; j < resolution; j++) {
			for (int k = 0; k < resolution; k++) {

				float3 temp_min, temp_max;
				temp_min.x = vmin.x + i * x_step;
				temp_min.y = vmin.y + j * y_step;
				temp_min.z = vmin.z + k * z_step;
				temp_max.x = temp_min.x + x_step;
				temp_max.y = temp_min.y + y_step;
				temp_max.z = temp_min.z + z_step;
				BoundingBox voxel(temp_min, temp_max);
				voxellist.push_back(voxel);
			}
		}
	}

	for (int i = 0; i < (int) triangle_list.size(); i++) {
		surface_voxelize(nor_vertex[triangle_list[i].a - 1],
				nor_vertex[triangle_list[i].b - 1],
				nor_vertex[triangle_list[i].c - 1], voxellist, resolution);

	}

	return voxellist;

}

float voxelization::length(float x, float y, float z) {
	float result;
	result = sqrt(x * x + y * y + z * z);
	return result;
}

int voxelization::quantize(BoundingBox min_corner, BoundingBox max_corner,
		float x, float y, float z, int resolution) {

	float3 vmin, vmax;
	float x_step, y_step, z_step;
	float q_x, q_y, q_z;
	int index;
	vmin = min_corner.getMin();
	vmax = max_corner.getMax();
	q_x = (int) (x - vmin.x) + min_corner.getCenter().x;
	q_y = (int) (y - vmin.y) + min_corner.getCenter().y;
	q_z = (int) (z - vmin.z) + min_corner.getCenter().z;

	if (x > vmax.x || y > vmax.y || z > vmax.z || x < vmin.x || y < vmin.y
			|| z < vmin.z)
		index = -1;
	else
		index = (int) (q_x - min_corner.getCenter().x)  * resolution
				* resolution
				+ (int) (q_y - min_corner.getCenter().y) * resolution
				+ (int) (q_z - min_corner.getCenter().z);
	return index;

}

void voxelization::getsample(const char *filename, int BW) {
	cout<<"## sampling by Voxelization"<<endl;
	int resolution = 2*R;
	vector<BoundingBox> voxellist;

	voxellist = voxelize(resolution);
	BoundingBox min_corner = voxellist[0];
	BoundingBox max_corner = voxellist[voxellist.size() - 1];

	int temp, flag;
    vector<int> templist;
	float phi, theta;
	string temp_fn(filename);
	string::size_type nbegin;
	string::size_type nend;

	nbegin = temp_fn.rfind('/',temp_fn.size());
	nend = temp_fn.rfind('.',temp_fn.size());
	if (nbegin == string::npos || nend == string::npos) {
		cout<<"wrong file name"<<endl;
		exit(0);
	}
	string objname = temp_fn.substr((nbegin+1),(nend-nbegin-1));
	objname = "./data/"+objname+".dat";
	ofstream outf(objname.c_str());

    templist.clear();
	for (float r = 1; r <= R; r++) {
		outf << "# " << r << endl;
		phi = 0;
		theta = 0;
		flag = 0;
		for (float k = 0; k < (2 * BW); k++) {
			for (float j = 0; j < (2 * BW); j++) {
				phi = (PI * k / BW);
				theta = (PI * (2 * j + 1) / 4 / BW);
				float x = r * sin(theta) * cos(phi);
				float y = r * cos(theta);
				float z = r * sin(theta) * sin(phi);
                
				temp = quantize(min_corner, max_corner, x, y, z,
						resolution);
                

				if (temp == -1)
                //    flaglist.push_back(0);
					flag = 0;
				else {

					if (voxellist[temp].getFlag() == true) {
                    
                        if(templist.empty()) {
                            flag = 1;
                            templist.push_back(temp);
                        }
                        else{
                        if(temp == templist[templist.size()-1]){
                            flag=0;
                        }
                        else{
                            templist.push_back(temp);
                            flag = 1;
                        }
                            
                        }      
                    
					} else
						flag = 0;
				}
				outf << flag << endl;
			}
		}
	

	}
	outf.close();
}

